

import com.ridgesoft.robotics.Servo;
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.intellibrain.IntelliBrainDigitalIO;

public class Movement implements Runnable{
	private static int MOTO_PORT = 1;
	private static int STEER_PORT = 2;
	private static int STOP = 45;
	private static int NEUTRAL = 50;
	private static int SHARP_LEFT = 25;
	private static int STEER_FWD = 65;
	private static int SHARP_RIGHT = 100;
	public static int DEFAULT_SPEED = 61;
	private int speed;
	private static final Movement instance = new Movement();
	private boolean boolStop = false;
  	private boolean killStatus = false;
	
	Servo steer;
	Servo moto;
	
	public Movement()
	{
		
	}
	
	public void stopAllMovement(){
		this.Stop();
		
	}
		
	public void init()
	{
		speed = DEFAULT_SPEED;
		moto = IntelliBrain.getServo(MOTO_PORT);
		steer = IntelliBrain.getServo(STEER_PORT);
		moto.setPosition(NEUTRAL);
		steer.setPosition(STEER_FWD);
		long time = System.currentTimeMillis();
        while (time + 2200 > System.currentTimeMillis()) {
        }
	}
	/*
	public Movement(int motoPort,int steerPort, int killPort)
	{
	}
	*/
	public void setSpeed(int value)
	{
		speed = value;
	}
	public int getSpeed()
	{
		return speed;
	}
	
	public void Left()
	{
		steer.setPosition(SHARP_LEFT);
	}
	public void Left2(int time)
	{
		try{
			Left();
			Thread.sleep(time);
		} catch (Throwable t) {
			t.printStackTrace();
		}
	}
	public void Right2(int time)
	{
		try{
			Right();
			Thread.sleep(time);
		} catch (Throwable t) {
			t.printStackTrace();
		}
	}

	public void Right()
	{
		steer.setPosition(SHARP_RIGHT);
	}
	
	public void straight(){
		steer.setPosition(STEER_FWD);
	}
	
	public void steerTo(int b){
		
	}

	public void Forward()
	{
		moto.setPosition(speed);
	}

	public void fwdSlow(){
		moto.setPosition(speed - 2);
	}
	
	public void Stop()
	{
		moto.setPosition(STOP);
		boolStop = true;
	}
	
	@Override
	public void run() {
		init();
		while (true) 
		{
			if(killStatus)
				Stop();
            Thread.yield();
            if(boolStop)
            	return;
        }
	}
	public static Movement getInstance() {
        return instance;
    }
}